/// 投影矩阵来回计算，建立理想的投影矩阵模型，反向计算出模型的参数
/// 根据给定参数计算出投影矩阵P，然后通过算法反计算出部分参数
#include <Eigen/Core>

#include <memath/MeVtkMath.h>
#include "CeresSolProjMatrix.h"

using namespace std;

/// 创建一个投影矩阵P
vtkSmartPointer<vtkMatrix4x4> createPMatrix(Eigen::Vector3d &center, Eigen::Vector3d &translation,
                                            double f, double dx, double dy, double u0, double v0,
                                            Eigen::Vector3d &anglesInDegree)
{

    Eigen::Vector3d anglesInRadian = anglesInDegree * M_PI/ 180.0;
    // 计算旋转R
    vtkSmartPointer<vtkMatrix4x4> m = MeVtkMath::rotateZYX(anglesInRadian[0], anglesInRadian[1], anglesInRadian[2]);

    // 计算t
    Eigen::Vector3d offset = MeVtkMath::multiplyPoint(m, center);
    m->Element[0][3] = translation[0]-offset[0];
    m->Element[1][3] = translation[1]-offset[1];
    m->Element[2][3] = translation[2]-offset[2];

    double fx = f/dx, fy = f/dy;
    m->Element[0][0] =m->Element[0][0]*fx + m->Element[2][0] * u0;
    m->Element[0][1] =m->Element[0][1]*fx + m->Element[2][1] * u0;
    m->Element[0][2] =m->Element[0][2]*fx + m->Element[2][2] * u0;
    m->Element[0][3] =m->Element[0][3]*fx + m->Element[2][3] * u0;

    m->Element[1][0] =m->Element[1][0]*fy + m->Element[2][0] * v0;
    m->Element[1][1] =m->Element[1][1]*fy + m->Element[2][1] * v0;
    m->Element[1][2] =m->Element[1][2]*fy + m->Element[2][2] * v0;
    m->Element[1][3] =m->Element[1][3]*fy + m->Element[2][3] * v0;

    return m;
}

/// 根据投影矩阵P = A*[R t] 反向计算 A
///
void computeCameraMatrix(vtkMatrix4x4 *m, double &u, double &v, double &f)
{
    //m->Print(std::cout);
    double *row1 = m->Element[0];
    double *row2 = m->Element[1];
    double *row3 = m->Element[2];

    double rx = atan2(row3[1], row3[2]);   // rx = atan2(a32, a33)
    double ry = atan2(-row3[0], sqrt(row3[1]*row3[1] + row3[2]*row3[2])); // ry = atan2(-a31, sqrt(a32^2+a33^2))

    double t3 = row3[3];

    //double u, v, f;
    double rz;

    CeresSolProjMatrix::solve(rx, ry,
            row1[0], row1[1], row1[2],
            row2[0], row2[1], row2[2],
            u, v, f, rz);


    cout << "Final :";
    cout << "u=" << u << " v=" << v << " ";
    cout << "f=" << f << " ";
    cout << "rx=" << rx * 180/M_PI << " ";
    cout << "ry=" << ry * 180/M_PI << " ";
    cout << "rz=" << rz * 180/M_PI << endl;
}

int main(int argc, char *argv[])
{
    cout << "===创建投影矩阵===" << endl;
    Eigen::Vector3d translation = {0, 0, 1000};
    Eigen::Vector3d center = {100, 100, 0};
    Eigen::Vector3d angleInDegree = {10, 20, 30};

    double f = 502;
    double dx = 1, dy = 1; // 1mm 对应1个像素
    double u0 = 300, v0 = 300;

    cout << "params: \n"
         << " f: " << f << " dx: " << dx << " dy: " << dy << endl;
    cout << "u0: " << u0 << " v0: " << v0 << endl;
    cout << "rotate angles(x,y,z): " << angleInDegree.transpose() << endl;
    vtkSmartPointer<vtkMatrix4x4> PMatrix = createPMatrix(center, translation, f, dx, dy, u0, v0,
                                                          angleInDegree);

    cout << "P:" << endl;
    PMatrix->Print(cout);

    double u1, v1, f1;
    computeCameraMatrix(PMatrix, u1, v1, f1);
    if (abs(u1-u0) > 1e-6 || abs(v1-v0) > 1e-6 || abs(f1-f) > 1e-6) {
        cerr << "Error params" << endl;
    }

    return 0;
}
